# Untitled - By: lpy13 - 周五 5月 15 2020

import sensor, image, time
import car

sensor.reset()
sensor.set_pixformat(sensor.RGB565)


sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)

ROI_ALL=(0,0,320,240)
ROI_LT=(30,30,15,15)
ROI_LB=(30,200,15,15)
ROI_RT=(270,30,15,15)
ROI_RB=(270,200,15,15)

block_box_color = (28, 55, 0, 16, -1, 28)

def expand_roi(roi):
     #set for QQVGA 160*120
    extra = 5
    win_size = (640, 480)
    (x, y, width, height) = roi
    new_roi = [x-extra, y-extra, width+2*extra, height+2*extra]

    if new_roi[0] < 0:
        new_roi[0] = 0
    if new_roi[1] < 0:
        new_roi[1] = 0
    if new_roi[2] > win_size[0]:
        new_roi[2] = win_size[0]
    if new_roi[3] > win_size[1]:
        new_roi[3] = win_size[1]

    return tuple(new_roi)

i = 0



clock = time.clock()

while(True):
    clock.tick()
    img = sensor.snapshot()
    img.replace(img,vflip=True, hmirror=True, transpose=False)


    #statistics=img.get_statistics(roi=ROI_LT)
    #color_lt_l=statistics.l_mode()
    #color_lt_a=statistics.a_mode()
    #color_lt_b=statistics.b_mode()
    #lab_lt = (color_lt_l,color_lt_a,color_lt_b)
    #rgb_lt = image.lab_to_rgb(lab_lt)

    #statistics=img.get_statistics(roi=ROI_LB)
    #color_lb_l=statistics.l_mode()
    #color_lb_a=statistics.a_mode()
    #color_lb_b=statistics.b_mode()
    #lab_lb = (color_lb_l,color_lb_a,color_lb_b)
    #rgb_lb = image.lab_to_rgb(lab_lb)

    #statistics=img.get_statistics(roi=ROI_RT)
    #color_rt_l=statistics.l_mode()
    #color_rt_a=statistics.a_mode()
    #color_rt_b=statistics.b_mode()
    #lab_rt = (color_rt_l,color_rt_a,color_rt_b)
    #rgb_rt = image.lab_to_rgb(lab_rt)

    #statistics=img.get_statistics(roi=ROI_RB)
    #color_rb_l=statistics.l_mode()
    #color_rb_a=statistics.a_mode()
    #color_rb_b=statistics.b_mode()
    #lab_rb = (color_rb_l,color_rb_a,color_rb_b)
    #rgb_rb = image.lab_to_rgb(lab_rb)

    #img.draw_rectangle(ROI_LT)
    #img.draw_rectangle(ROI_LB)
    #img.draw_rectangle(ROI_RT)
    #img.draw_rectangle(ROI_RB)
    img.draw_line(0,200,320,200,(0,0,255),thinkness = 1)


    find_block = img.find_blobs([block_box_color], area_threshold=10000)

    time.sleep(200)
    car.run(20,24)
    time.sleep(100)
    car.run(0,0)

    if find_block:
    #如果找到了目标颜色
        for blob in find_block:
        #迭代找到的目标颜色区域
            is_line = False
            max_line = None
            max_length = -1
            is_block = 0

            new_roi = expand_roi(blob.rect())


            # Draw a rect around the blob.
            img.draw_rectangle(new_roi) # rect
            #img.draw_rectangle(blob.rect()) # rect
            #用矩形标记出目标颜色区域
            img.draw_cross(blob[5], blob[6]) # cx, cy

            ltx = new_roi[0]
            lty = new_roi[1]

            lbx = new_roi[0]
            lby = new_roi[1] + new_roi[3]

            rtx = new_roi[0] + new_roi[2]
            rty = new_roi[1]

            rbx = new_roi[0] + new_roi[2]
            rby = new_roi[1] + new_roi[3]

            print("ltx=",ltx,"lty=",lty)
            print("lbx=",lbx,"lby=",lby)
            print("rtx=",rtx,"rty=",rty)
            print("rbx=",rbx,"rby=",rby)
            print("\n")
            if lby > 200 and (320-rtx)<ltx :
                car.turn_left(70)
                print("turn_left")
            if lby > 200 and (320-rtx)>ltx :
                car.turn_left(70)
                print("turn_right")


            #for c in img.find_lines(x_stride=5, y_stride=5, threshold=1000, theta_margin=100, rho_margin=100):
                #is_line = True
                ## img.draw_circle(c.x(), c.y(), c.r(), color = (255, 255, 255))
                #if c.length() > max_length:
                    #max_length = c.length()
                    #max_line = c
            #if is_line:
                # 如果有对应颜色的圆形 标记外框
                ## Draw a rect around the blob.
                #img.draw_rectangle(new_roi) # rect
                #img.draw_rectangle(blob.rect()) # rect
                ##用矩形标记出目标颜色区域
                #img.draw_cross(blob[5], blob[6]) # cx, cy
                #img.draw_line(c.x1(), c.y1(), c.x2(), c.y2(), color = (0, 255, 0),thickness = 1)
                #距离约为15cm-----190

                ##if c.y1()<220 | c.y1()>200 | c.y2()<220 | c.y2()>200:
                #if c.y1()>170 | c.y2()>170:
                    #car.turn_right(70)
                    #print("car.turn_right")

